// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PoseFeedbackStateTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Hardware.DAC
{
    using System;

    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// The Drive state feedback unit test class
    /// </summary>
    [TestClass]
    public class PoseFeedbackStateTest
    {
        /// <summary>
        /// Default noiseless odometry
        /// </summary>
        private PoseNoiseModelParams noiseDisabled = new PoseNoiseModelParams() { AngleFromInitialToTranslationStdInRads = 0.001, TranslationStdDevInMeters = 0.001, AngleFromTranslationToFinalStdInRads = 0.001, IgnoreNoiseParams = true };

        /// <summary>
        /// Default noisy odometry
        /// </summary>
        private PoseNoiseModelParams noisyPose = new PoseNoiseModelParams() { AngleFromInitialToTranslationStdInRads = 0.001, TranslationStdDevInMeters = 0.001, AngleFromTranslationToFinalStdInRads = 0.001, IgnoreNoiseParams = false };

        /// <summary>
        /// Noisy heading only, no wheel noise.
        /// </summary>
        private PoseNoiseModelParams noisyHeading = new PoseNoiseModelParams() { AngleFromInitialToTranslationStdInRads = 0.001, TranslationStdDevInMeters = 0.0, AngleFromTranslationToFinalStdInRads = 0.0, IgnoreNoiseParams = false };

        /// <summary>
        /// Noisy wheel only, no heading noise.
        /// </summary>
        private PoseNoiseModelParams noisyTranslation = new PoseNoiseModelParams() { AngleFromInitialToTranslationStdInRads = 0.0, TranslationStdDevInMeters = 0.001, AngleFromTranslationToFinalStdInRads = 0.001, IgnoreNoiseParams = false };

        /// <summary>
        /// Heading noisier than wheel. We should trust wheels more.
        /// </summary>
        private PoseNoiseModelParams headingNoisierThanTranslation = new PoseNoiseModelParams() { AngleFromInitialToTranslationStdInRads = 0.01, TranslationStdDevInMeters = 0.001, AngleFromTranslationToFinalStdInRads = 0.001, IgnoreNoiseParams = false };

        /// <summary>
        /// Wheels noisier than heading. We should trust heading more.
        /// </summary>
        private PoseNoiseModelParams translationNoisierThanHeading = new PoseNoiseModelParams() { AngleFromInitialToTranslationStdInRads = 0.001, TranslationStdDevInMeters = 0.01, AngleFromTranslationToFinalStdInRads = 0.01, IgnoreNoiseParams = false };

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseFeedbackStateRotateClean()
        {
            PoseFeedbackState pfsPrevious = new PoseFeedbackState(new Pose2D(0, 0, 0), new PoseNoiseModelParams());
            PoseFeedbackState pfs = new PoseFeedbackState(new Pose2D(0, 0, MathConstants.PIOverFour), new PoseNoiseModelParams());

            Pose2D pose = pfs.GetUpdatedPose(new Pose2D(0, 0, 0), pfsPrevious);
            Assert.IsTrue(pose.X == 0.0 && pose.Y == 0.0 && pose.Heading == MathConstants.PIOverFour);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseFeedbackStateForwardClean()
        {
            PoseFeedbackState pfsPrevious = new PoseFeedbackState(new Pose2D(0, 0, 0), new PoseNoiseModelParams());
            PoseFeedbackState pfs = new PoseFeedbackState(new Pose2D(1, 1, 0), new PoseNoiseModelParams());

            Pose2D pose = pfs.GetUpdatedPose(new Pose2D(0, 0, 0), pfsPrevious);            
            Assert.AreEqual(1.0, pose.X, MathConstants.ErrorEpsilon);
            Assert.AreEqual(1.0, pose.Y, MathConstants.ErrorEpsilon);
            Assert.AreEqual(0, pose.Heading, MathConstants.ErrorEpsilon);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseFeedbackStateBackwardClean()
        {
            PoseFeedbackState pfsPrevious = new PoseFeedbackState(new Pose2D(0, 0, 0), new PoseNoiseModelParams());
            PoseFeedbackState pfs = new PoseFeedbackState(new Pose2D(-1, -1, 0), new PoseNoiseModelParams());

            Pose2D pose = pfs.GetUpdatedPose(new Pose2D(0, 0, 0), pfsPrevious);
            Assert.AreEqual(-1.0, pose.X, MathConstants.ErrorEpsilon);
            Assert.AreEqual(-1.0, pose.Y, MathConstants.ErrorEpsilon);
            Assert.AreEqual(0, pose.Heading, MathConstants.ErrorEpsilon);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseFeedbackStateForwardAndRotate()
        {
            PoseFeedbackState pfsPrevious = new PoseFeedbackState(new Pose2D(0, 0, 0), new PoseNoiseModelParams());
            PoseFeedbackState pfs = new PoseFeedbackState(new Pose2D(1, 1, Math.PI), new PoseNoiseModelParams());

            Pose2D pose = pfs.GetUpdatedPose(new Pose2D(0, 0, 0), pfsPrevious);            
            Assert.AreEqual(1.0, pose.X, MathConstants.ErrorEpsilon);
            Assert.AreEqual(1.0, pose.Y, MathConstants.ErrorEpsilon);
            Assert.AreEqual(Math.PI, pose.Heading, MathConstants.ErrorEpsilon);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseFeedbackStateRotateCleanWithPreviousHeading()
        {
            PoseFeedbackState pfsPrevious = new PoseFeedbackState(new Pose2D(0, 0, -MathConstants.PIOverFour), new PoseNoiseModelParams());
            PoseFeedbackState pfs = new PoseFeedbackState(new Pose2D(0, 0, MathConstants.PIOverFour), new PoseNoiseModelParams());

            Pose2D pose = pfs.GetUpdatedPose(new Pose2D(0, 0, 0), pfsPrevious);
            Assert.IsTrue(pose.X == 0.0 && pose.Y == 0.0 && pose.Heading == MathConstants.PIOverTwo);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseFeedbackStateRotateCleanVariedSample()
        {
            PoseFeedbackState pfsPrevious = new PoseFeedbackState(new Pose2D(0, 0, 0), new PoseNoiseModelParams());
            PoseFeedbackState pfs = new PoseFeedbackState(new Pose2D(0, 0, MathConstants.PIOverFour), new PoseNoiseModelParams());

            Pose2D pose = pfs.GetUpdatedPose(new Pose2D(1.0, 1.0, MathConstants.PIOverFour), pfsPrevious);
            Assert.IsTrue(pose.X == 1.0 && pose.Y == 1.0 && pose.Heading == MathConstants.PIOverTwo);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseFeedbackStateRotateCleanVariedSampleIgnoreNoise()
        {
            PoseFeedbackState pfsPrevious = new PoseFeedbackState(new Pose2D(0, 0, 0), this.noiseDisabled);
            PoseFeedbackState pfs = new PoseFeedbackState(new Pose2D(0, 0, MathConstants.PIOverFour), this.noiseDisabled);

            Pose2D pose = pfs.SamplePose(new Pose2D(1.0, 1.0, MathConstants.PIOverFour), pfsPrevious);
            Assert.IsTrue(pose.X == 1.0 && pose.Y == 1.0 && pose.Heading == MathConstants.PIOverTwo);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseFeedbackStateNoisyPoseNoMotionNoRotationShouldResultInNoNoise()
        {
            PoseFeedbackState pfsPrevious = new PoseFeedbackState(new Pose2D(0, 0, 0), this.noisyPose);
            PoseFeedbackState pfs = new PoseFeedbackState(new Pose2D(0, 0, 0), this.noisyPose);

            Pose2D pose = pfs.SamplePose(new Pose2D(0, 0, 0), pfsPrevious);            
            Assert.AreEqual(0.0, pose.X, MathConstants.ErrorEpsilon);
            Assert.AreEqual(0.0, pose.Y, MathConstants.ErrorEpsilon);
            Assert.AreEqual(0.0, pose.Heading, MathConstants.ErrorEpsilon);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseFeedbackStateFitnessGetPoseShouldBeNaN()
        {
            PoseFeedbackState pfsPrevious = new PoseFeedbackState(new Pose2D(0, 0, 0), this.noisyPose);
            PoseFeedbackState pfs = new PoseFeedbackState(new Pose2D(1, 2, Math.PI), this.noisyPose);
            double fitness;
            Pose2D pose = pfs.GetUpdatedPose(new Pose2D(0, 0, 0), pfsPrevious, out fitness);
            Assert.IsTrue(double.IsNaN(fitness));            
            Assert.AreEqual(1.0, pose.X, MathConstants.ErrorEpsilon);
            Assert.AreEqual(2.0, pose.Y, MathConstants.ErrorEpsilon);
            Assert.AreEqual(Math.PI, pose.Heading, MathConstants.ErrorEpsilon);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseFeedbackStateFitnessSamplePoseShouldBeNaN()
        {
            PoseFeedbackState pfsPrevious = new PoseFeedbackState(new Pose2D(0, 0, 0), this.noisyPose);
            PoseFeedbackState pfs = new PoseFeedbackState(new Pose2D(0, 0, 0), this.noisyPose);
            double fitness;
            Pose2D pose = pfs.SamplePose(new Pose2D(0, 0, 0), pfsPrevious, out fitness);
            Assert.IsTrue(double.IsNaN(fitness));
            Assert.IsTrue(pose.X != 1.0 && pose.Y != 2.0 && pose.Heading != Math.PI);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseFeedbackStateNoisyHeadingOnlyTranslationNoRotationShouldResultInTranslationAndRotationNoise()
        {
            PoseFeedbackState pfsPrevious = new PoseFeedbackState(new Pose2D(0, 0, 0), this.noisyPose);
            PoseFeedbackState pfs = new PoseFeedbackState(new Pose2D(1, 1, 0), this.noisyPose);

            Pose2D pose = pfs.SamplePose(new Pose2D(0, 0, 0), pfsPrevious);
            Assert.IsTrue(pose.X != 1.0 && pose.Y != 1.0 && pose.Heading != 0);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseFeedbackStateNoisyPoseNoTranslationOnlyRotationShouldResultInOnlyRotationNoise()
        {
            PoseFeedbackState pfsPrevious = new PoseFeedbackState(new Pose2D(0, 0, 0), this.noisyPose);
            PoseFeedbackState pfs = new PoseFeedbackState(new Pose2D(0, 0, MathConstants.PIOverFour), this.noisyPose);

            Pose2D pose = pfs.SamplePose(new Pose2D(0, 0, MathConstants.PIOverFour), pfsPrevious);
            Assert.IsTrue(pose.X == 0.0 && pose.Y == 0.0 && pose.Heading != MathConstants.PIOverTwo);
            Assert.AreEqual(MathConstants.PIOverTwo, pose.Heading, 100 * this.noisyPose.AngleFromInitialToTranslationStdInRads);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseFeedbackStateNoisyPoseTranslationAndRotationShouldResultInTranslationAndRotationNoise()
        {
            PoseFeedbackState pfsPrevious = new PoseFeedbackState(new Pose2D(0, 0, 0), this.noisyPose);
            PoseFeedbackState pfs = new PoseFeedbackState(new Pose2D(2, 3, MathConstants.PIOverFour), this.noisyPose);

            Pose2D pose = pfs.SamplePose(new Pose2D(0, 0, MathConstants.PIOverFour), pfsPrevious);
            Assert.IsTrue(pose.X != 2.0 && pose.Y != 3.0 && pose.Heading != MathConstants.PIOverTwo);
            Assert.AreEqual(2, pose.X, 100 * this.noisyPose.TranslationStdDevInMeters);
            Assert.AreEqual(3, pose.Y, 100 * this.noisyPose.AngleFromTranslationToFinalStdInRads);
            Assert.AreEqual(MathConstants.PIOverTwo, pose.Heading, 100 * this.noisyPose.AngleFromInitialToTranslationStdInRads);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseFeedbackStateNoisyPoseTranslationAndRotationShouldResultInTranslationAndRotationNoiseForSampleDeltaPose()
        {
            PoseFeedbackState pfsPrevious = new PoseFeedbackState(new Pose2D(0, 0, 0), this.noisyPose);
            PoseFeedbackState pfs = new PoseFeedbackState(new Pose2D(2, 3, MathConstants.PIOverFour), this.noisyPose);

            Pose2D pose = pfs.SampleDeltaPose(pfsPrevious);
            Assert.IsTrue(pose.X != 2.0 && pose.Y != 3.0 && pose.Heading != MathConstants.PIOverFour);
            Assert.AreEqual(2, pose.X, 100 * this.noisyPose.TranslationStdDevInMeters);
            Assert.AreEqual(3, pose.Y, 100 * this.noisyPose.AngleFromTranslationToFinalStdInRads);
            Assert.AreEqual(MathConstants.PIOverFour, pose.Heading, 100 * this.noisyPose.AngleFromInitialToTranslationStdInRads);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseFeedbackStateNoisyPoseTranslationAndRotationShouldResultInTranslationAndRotationNoiseForGetMeanDeltaPose()
        {
            PoseFeedbackState pfsPrevious = new PoseFeedbackState(new Pose2D(0, 0, 0), this.noisyPose);
            PoseFeedbackState pfs = new PoseFeedbackState(new Pose2D(2, 3, MathConstants.PIOverFour), this.noisyPose);

            Pose2D pose = pfs.GetMeanDeltaPose(pfsPrevious);            
            Assert.AreEqual(2, pose.X);
            Assert.AreEqual(3, pose.Y);
            Assert.AreEqual(MathConstants.PIOverFour, pose.Heading);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentException))]
        public void IncorrectPreviousPoseShouldResultInArgumentExceptionForSamplePose()
        {
            DriveFeedbackState pfsPrevious = new DriveFeedbackState(0, 0, 1, new OdometryNoiseModelParams(), null);
            PoseFeedbackState pfs = new PoseFeedbackState(new Pose2D(2, 3, MathConstants.PIOverFour), this.noisyPose);

            Pose2D pose = pfs.SamplePose(new Pose2D(0, 0, MathConstants.PIOverFour), pfsPrevious);         
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentException))]
        public void IncorrectPreviousPoseShouldResultInArgumentExceptionForGetPose()
        {
            DriveFeedbackState pfsPrevious = new DriveFeedbackState(0, 0, 1, new OdometryNoiseModelParams(), null);
            PoseFeedbackState pfs = new PoseFeedbackState(new Pose2D(2, 3, MathConstants.PIOverFour), this.noisyPose);

            Pose2D pose = pfs.GetUpdatedPose(new Pose2D(0, 0, MathConstants.PIOverFour), pfsPrevious);         
        }      
    }
}
